#pragma once
#ifndef PRM_H
#define PRM_H

#include "basic_typedef.h"
#include "Graph.h"
#include "CollisionDetector.h"
#include "Sampler.h"
#include "Kd_tree_d.h"

#define POINT_DISTANCE_METRIC 1

// Should be used with the Graph wrapper
struct Less_than_int
{
  bool operator()(const int& p1, 
                  const int& p2) const 
  {
    return (p1 < p2);
  }
};

Point_d to_point_d(const vector<Conf>& p) {
    vector<double> v(4);
    v[0] = CGAL::to_double(p[0].x());
    v[1] = CGAL::to_double(p[0].y());
    v[2] = CGAL::to_double(p[1].x());
    v[3] = CGAL::to_double(p[1].y());

    return Point_d(v.size(), v.begin(), v.end());
}

#define EPS 1

class Prm
{
public:
	/*
		IMPORTANT:	The main code of the PRM algorithm should be placed in this file.
					You may change its structure, but make sure you that it remains consistent
					with the run() operation in Planner.h
	*/

	Prm (	int number_vertices, 
        int k_nearest, 
        CollisionDetector* col, 
        Sampler* sampler,
        vector<Conf> start, vector<Conf> target) : 
		m_number_vertices(number_vertices), m_k_nearest(k_nearest), 
		  m_col(col), m_sampler(sampler),m_graph(0),
		  m_start(to_point_d(start)), m_target(to_point_d(target))
	{
    m_graph = new Graph<int, Less_than_int>(0, true);
    m_kd_tree = new Kd_tree_d<Kernel_d>;
	}

	~Prm()
	{
    delete m_graph;
		delete m_kd_tree;
	}
	
	//	This operation is supposed to generate the roadmap (sample configurations
	//	and connect them.
	double pointDistance(Point_d a, Point_d b) {
#if POINT_DISTANCE_METRIC == 1
		//Point metric 1 : The distance between the 4D points
		return sqrt((a - b).squared_length());
#else
		//Point metric 2 : The maximum distance between the two robot configurations
		Point_d diff = (b-a).to_point();
		Point_2 diffA = Point_2(diff[0], diff[1]);
		Point_2 diffB = Point_2(diff[2], diff[3]);
		double distA = sqrt(CGAL::to_double((diffA - CGAL::ORIGIN).squared_length()));
		double distB = sqrt(CGAL::to_double((diffB - CGAL::ORIGIN).squared_length()));
		return std::max(distA, distB);
#endif
	}

	void			generate_roadmap() {
    m_points.push_back(m_start);
    m_points.push_back(m_target);
    m_graph->add_vertex(0);
    m_graph->add_vertex(1);
    for (size_t i = 0; i < m_number_vertices; ++i) {
      m_points.push_back(m_sampler->generate_sample());
      m_graph->add_vertex(i + 2);
    }
    cout << "Done choosing points" << endl;
    m_kd_tree->insert(m_points.begin(), m_points.end());
    time_t start = time(NULL);
    int count = 0;
    int total_count = 0;
    for (size_t i = 0; i < m_points.size(); ++i) {
      //cout << "local_planner() for point #" << i << endl;
      vector<Point_d> nn;
      m_kd_tree->k_nearest_neighbors(m_points[i], m_k_nearest+1, std::back_inserter(nn));
      for (vector<Point_d>::const_iterator iter = nn.begin(); iter != nn.end(); ++iter) {
        total_count++;
        if (m_col->local_planner(m_points[i], *iter, EPS)) {
          count++;
          int j = std::distance(m_points.begin(), std::find(m_points.begin(), m_points.end(), *iter));
          m_graph->add_edge(i, j, pointDistance(m_points[i], *iter));
        }
      }
    }
    time_t time_took = time(NULL) - start;
    cout << "Took " << time_took << "s, " << static_cast<double>(time_took) / total_count << "s, for total of " << total_count << " runs with " << count << " trues" << endl;

    cout << "done" << endl;
	}

	//	Returns a point path from start to target.
	//	If a path doesn't exist, returns empty vector.
	//	TODO: Implement this function.
	vector<vector<Conf>	>		retrieve_path() {
		vector<vector<Conf> > retVal;
    if (!m_graph->is_in_same_cc(0, 1)) {
      cout << "Couldn't find solution :(" << endl;
      return retVal;
    }
    list<int> path;
    m_graph->find_path(0, 1, path);
    for (list<int>::const_iterator iter = path.begin(); iter != path.end(); ++iter) {
      vector<Conf> robot_confs(2);
      robot_confs[0] = Point_2(m_points[*iter][0], m_points[*iter][1]);
      robot_confs[1] = Point_2(m_points[*iter][2], m_points[*iter][3]);
      retVal.push_back(robot_confs);
    }
		return retVal;
	}

private:

	int									m_number_vertices;	// number of sampled vertices
	int									m_k_nearest;		// maximal degree of vertex in roadmap
	CollisionDetector*					m_col;
	Sampler*							m_sampler;
	
	Graph<int,Less_than_int>*			m_graph;			//	Graph structure
	Kd_tree_d<Kernel_d>*					m_kd_tree;			//	Kd-tree for nearest neighbor search

	Point_d 							m_start,m_target;	//	Start and target configurations

  vector<Point_d > m_points;
};

#endif
